% 北邮沙河校区21个配送点TSP优化（基于文档1模型）
clear; clc; 
%rng(0); % 固定随机种子，结果可复现
% 使用 rng('shuffle') 基于当前时间初始化随机数种子
rng('shuffle'); 

%% 1. 构建距离矩阵（仅含A和配送点1~21，共22个节点：A=1，配送点1~21=2~22）
D = zeros(22, 22);

% ========== 行1（节点1=A） ==========
D(1,1)=0;    D(1,2)=216;   D(1,3)=119;   D(1,4)=100;   D(1,5)=196;   D(1,6)=55;    
D(1,7)=177;  D(1,8)=235;   D(1,9)=270;   D(1,10)=356;  D(1,11)=413;  D(1,12)=71;   
D(1,13)=322; D(1,14)=460;  D(1,15)=407;  D(1,16)=475;  D(1,17)=553;  D(1,18)=401;  
D(1,19)=393; D(1,20)=453;  D(1,21)=393;  D(1,22)=698;

% ========== 行2（节点2=配送点1） ==========
D(2,1)=216;  D(2,2)=0;     D(2,3)=96;    D(2,4)=87;    D(2,5)=160;   D(2,6)=240;   
D(2,7)=197;  D(2,8)=317;   D(2,9)=375;   D(2,10)=410;  D(2,11)=496;  D(2,12)=553;  
D(2,13)=277; D(2,14)=528;  D(2,15)=666;  D(2,16)=613;  D(2,17)=681;  D(2,18)=759;  
D(2,19)=607; D(2,20)=621;  D(2,21)=659;  D(2,22)=904;

% ========== 行3（节点3=配送点2） ==========
D(3,1)=119;  D(3,2)=96;    D(3,3)=0;     D(3,4)=97;    D(3,5)=170;   D(3,6)=144;   
D(3,7)=207;  D(3,8)=221;   D(3,9)=279;   D(3,10)=314;  D(3,11)=400;  D(3,12)=457;  
D(3,13)=287; D(3,14)=441;  D(3,15)=579;  D(3,16)=526;  D(3,17)=594;  D(3,18)=672;  
D(3,19)=520; D(3,20)=631;  D(3,21)=572;  D(3,22)=817;

% ========== 行4（节点4=配送点3） ==========
D(4,1)=100;  D(4,2)=87;    D(4,3)=97;    D(4,4)=0;     D(4,5)=73;    D(4,6)=241;   
D(4,7)=110;  D(4,8)=232;   D(4,9)=289;   D(4,10)=325;  D(4,11)=411;  D(4,12)=468;  
D(4,13)=190; D(4,14)=441;  D(4,15)=579;  D(4,16)=526;  D(4,17)=594;  D(4,18)=672;  
D(4,19)=520; D(4,20)=534;  D(4,21)=572;  D(4,22)=817;

% ========== 行5（节点5=配送点4） ==========
D(5,1)=196;  D(5,2)=160;   D(5,3)=170;   D(5,4)=73;    D(5,5)=0;     D(5,6)=232;   
D(5,7)=91;   D(5,8)=213;   D(5,9)=271;   D(5,10)=306;  D(5,11)=392;  D(5,12)=449;  
D(5,13)=171; D(5,14)=422;  D(5,15)=560;  D(5,16)=507;  D(5,17)=575;  D(5,18)=653;  
D(5,19)=501; D(5,20)=515;  D(5,21)=553;  D(5,22)=798;

% ========== 行6（节点6=配送点5） ==========
D(6,1)=55;   D(6,2)=240;   D(6,3)=144;   D(6,4)=241;   D(6,5)=232;   D(6,6)=0;    
D(6,7)=141;  D(6,8)=119;   D(6,9)=177;   D(6,10)=212;  D(6,11)=298;  D(6,12)=355;  
D(6,13)=267; D(6,14)=339;  D(6,15)=477;  D(6,16)=424;  D(6,17)=492;  D(6,18)=570;  
D(6,19)=418; D(6,20)=619;  D(6,21)=460;  D(6,22)=715;

% ========== 行7（节点7=配送点6） ==========
D(7,1)=177;  D(7,2)=197;   D(7,3)=207;   D(7,4)=110;   D(7,5)=91;    D(7,6)=141;   
D(7,7)=0;    D(7,8)=122;   D(7,9)=180;   D(7,10)=215;  D(7,11)=301;  D(7,12)=358;  
D(7,13)=126; D(7,14)=342;  D(7,15)=480;  D(7,16)=427;  D(7,17)=495;  D(7,18)=573;  
D(7,19)=421; D(7,20)=470;  D(7,21)=508;  D(7,22)=718;

% ========== 行8（节点8=配送点7） ==========
D(8,1)=235;  D(8,2)=317;   D(8,3)=221;   D(8,4)=232;   D(8,5)=213;   D(8,6)=119;   
D(8,7)=122;  D(8,8)=0;     D(8,9)=58;    D(8,10)=93;   D(8,11)=179;  D(8,12)=236;  
D(8,13)=248; D(8,14)=320;  D(8,15)=363;  D(8,16)=405;  D(8,17)=466;  D(8,18)=470;  
D(8,19)=399; D(8,20)=600;  D(8,21)=441;  D(8,22)=615;

% ========== 行9（节点9=配送点8） ==========
D(9,1)=270;  D(9,2)=375;   D(9,3)=279;   D(9,4)=289;   D(9,5)=271;   D(9,6)=177;   
D(9,7)=180;  D(9,8)=58;    D(9,9)=0;     D(9,10)=35;   D(9,11)=121;  D(9,12)=178;  
D(9,13)=306; D(9,14)=378;  D(9,15)=305;  D(9,16)=463;  D(9,17)=408;  D(9,18)=412;  
D(9,19)=457; D(9,20)=658;  D(9,21)=499;  D(9,22)=557;

% ========== 行10（节点10=配送点9） ==========
D(10,1)=356; D(10,2)=410;  D(10,3)=314;  D(10,4)=325;  D(10,5)=306;  D(10,6)=212;  
D(10,7)=215; D(10,8)=93;   D(10,9)=35;   D(10,10)=0;   D(10,11)=86;  D(10,12)=143; 
D(10,13)=341; D(10,14)=413; D(10,15)=275; D(10,16)=446; D(10,17)=378; D(10,18)=377; 
D(10,19)=492; D(10,20)=693; D(10,21)=544; D(10,22)=522;

% ========== 行11（节点11=配送点10） ==========
D(11,1)=413; D(11,2)=496;  D(11,3)=400;  D(11,4)=411;  D(11,5)=392;  D(11,6)=298;  
D(11,7)=301; D(11,8)=179;  D(11,9)=121;  D(11,10)=86;  D(11,11)=0;   D(11,12)=57;  
D(11,13)=427; D(11,14)=499; D(11,15)=361; D(11,16)=532; D(11,17)=464; D(11,18)=381; 
D(11,19)=578; D(11,20)=779; D(11,21)=630; D(11,22)=454;

% ========== 行12（节点12=配送点11） ==========
D(12,1)=71;  D(12,2)=553;  D(12,3)=457;  D(12,4)=468;  D(12,5)=449;  D(12,6)=355;  
D(12,7)=358; D(12,8)=236;  D(12,9)=178;  D(12,10)=143; D(12,11)=57;  D(12,12)=0;  
D(12,13)=484; D(12,14)=556; D(12,15)=418; D(12,16)=589; D(12,17)=621; D(12,18)=438; 
D(12,19)=635; D(12,20)=836; D(12,21)=687; D(12,22)=511;

% ========== 行13（节点13=配送点12） ==========
D(13,1)=322; D(13,2)=277;  D(13,3)=287;  D(13,4)=190;  D(13,5)=171;  D(13,6)=267;  
D(13,7)=126; D(13,8)=248;  D(13,9)=306;  D(13,10)=341; D(13,11)=427; D(13,12)=484; 
D(13,13)=0;   D(13,14)=251; D(13,15)=389; D(13,16)=336; D(13,17)=404; D(13,18)=482; 
D(13,19)=330; D(13,20)=344; D(13,21)=382; D(13,22)=627;

% ========== 行14（节点14=配送点13） ==========
D(14,1)=460; D(14,2)=528;  D(14,3)=441;  D(14,4)=441;  D(14,5)=422;  D(14,6)=339;  
D(14,7)=342; D(14,8)=320;  D(14,9)=378;  D(14,10)=413; D(14,11)=499; D(14,12)=556; 
D(14,13)=251; D(14,14)=0;   D(14,15)=256; D(14,16)=85;  D(14,17)=153; D(14,18)=231; 
D(14,19)=79;  D(14,20)=280; D(14,21)=121; D(14,22)=376;

% ========== 行15（节点15=配送点14） ==========
D(15,1)=407; D(15,2)=666;  D(15,3)=579;  D(15,4)=579;  D(15,5)=560;  D(15,6)=477;  
D(15,7)=480; D(15,8)=363;  D(15,9)=305;  D(15,10)=275; D(15,11)=361; D(15,12)=418; 
D(15,13)=389; D(15,14)=256; D(15,15)=0;   D(15,16)=171; D(15,17)=103; D(15,18)=107; 
D(15,19)=225; D(15,20)=426; D(15,21)=277; D(15,22)=252;

% ========== 行16（节点16=配送点15） ==========
D(16,1)=475; D(16,2)=613;  D(16,3)=526;  D(16,4)=526;  D(16,5)=507;  D(16,6)=424;  
D(16,7)=427; D(16,8)=405;  D(16,9)=463;  D(16,10)=446; D(16,11)=532; D(16,12)=589; 
D(16,13)=336; D(16,14)=85;  D(16,15)=171; D(16,16)=0;   D(16,17)=68;  D(16,18)=146; 
D(16,19)=54;  D(16,20)=255; D(16,21)=106; D(16,22)=291;

% ========== 行17（节点17=配送点16） ==========
D(17,1)=553; D(17,2)=681;  D(17,3)=594;  D(17,4)=594;  D(17,5)=575;  D(17,6)=492;  
D(17,7)=495; D(17,8)=466;  D(17,9)=408;  D(17,10)=378; D(17,11)=464; D(17,12)=621; 
D(17,13)=404; D(17,14)=153; D(17,15)=103; D(17,16)=68;  D(17,17)=0;   D(17,18)=78;  
D(17,19)=122; D(17,20)=323; D(17,21)=174; D(17,22)=223;

% ========== 行18（节点18=配送点17） ==========
D(18,1)=401; D(18,2)=759;  D(18,3)=672;  D(18,4)=672;  D(18,5)=653;  D(18,6)=570;  
D(18,7)=573; D(18,8)=470;  D(18,9)=412;  D(18,10)=377; D(18,11)=381; D(18,12)=438; 
D(18,13)=482; D(18,14)=231; D(18,15)=107; D(18,16)=146; D(18,17)=78;  D(18,18)=0;  
D(18,19)=200; D(18,20)=401; D(18,21)=252; D(18,22)=145;

% ========== 行19（节点19=配送点18） ==========
D(19,1)=393; D(19,2)=607;  D(19,3)=520;  D(19,4)=520;  D(19,5)=501;  D(19,6)=418;  
D(19,7)=421; D(19,8)=399;  D(19,9)=457;  D(19,10)=492; D(19,11)=578; D(19,12)=635; 
D(19,13)=330; D(19,14)=79;  D(19,15)=225; D(19,16)=54;  D(19,17)=122; D(19,18)=200; 
D(19,19)=0;   D(19,20)=266; D(19,21)=117; D(19,22)=345;

% ========== 行20（节点20=配送点19） ==========
D(20,1)=453; D(20,2)=621;  D(20,3)=631;  D(20,4)=534;  D(20,5)=515;  D(20,6)=619;  
D(20,7)=470; D(20,8)=600;  D(20,9)=658;  D(20,10)=693; D(20,11)=779; D(20,12)=836; 
D(20,13)=344; D(20,14)=280; D(20,15)=426; D(20,16)=255; D(20,17)=323; D(20,18)=401; 
D(20,19)=266; D(20,20)=0;   D(20,21)=278; D(20,22)=485;

% ========== 行21（节点21=配送点20） ==========
D(21,1)=393; D(21,2)=659;  D(21,3)=572;  D(21,4)=572;  D(21,5)=553;  D(21,6)=460;  
D(21,7)=508; D(21,8)=441;  D(21,9)=499;  D(21,10)=544; D(21,11)=630; D(21,12)=687; 
D(21,13)=382; D(21,14)=121; D(21,15)=277; D(21,16)=106; D(21,17)=174; D(21,18)=252; 
D(21,19)=117; D(21,20)=278; D(21,21)=0;   D(21,22)=397;

% ========== 行22（节点22=配送点21） ==========
D(22,1)=698; D(22,2)=904;  D(22,3)=817;  D(22,4)=817;  D(22,5)=798;  D(22,6)=715;  
D(22,7)=718; D(22,8)=615;  D(22,9)=557;  D(22,10)=522; D(22,11)=454; D(22,12)=511; 
D(22,13)=627; D(22,14)=376; D(22,15)=252; D(22,16)=291; D(22,17)=223; D(22,18)=145; 
D(22,19)=345; D(22,20)=485; D(22,21)=397; D(22,22)=0;

% ========== 对称填充下三角（确保距离矩阵对称） ==========
for i = 1:22
    for j = 1:i-1
        D(i,j) = D(j,i);
    end
end

%% 2. 生成随机订餐量与优先级（文档3.3.2节参数）
orders = randi([10, 100], 1, 21); % 配送点1-21的订餐量
min_order_threshold = 30;
% 对10号点（索引11）进行优先级补偿
if orders(10) < min_order_threshold
    orders(10) = min_order_threshold; % 强制订餐量不低于30
    fprintf('10号点订餐量低，已提升至%d\n', min_order_threshold);
end
priority = orders / max(orders);   % 归一化优先级（0-1）

%% 3. 蚁群算法参数优化（增强探索性）
m = 150;          % 增加蚂蚁数量到150
alpha = 1.8;      % 降低信息素权重
beta = 2.5;       % 平衡启发式权重
rho = 0.05;       % 提高信息素蒸发率
max_iter = 1000;  % 增加迭代次数
Q = 120;          % 信息素增量常量

%% 4. 启发式信息计算（融合距离与优先级，文档3.3.3节）
city_num = 22; % 补充city_num定义
eta = zeros(city_num, city_num);
for i = 1:city_num
    for j = 1:city_num
        if i == j
            eta(i,j) = 0;
        else
            if j == 1 % A点无优先级
                eta(i,j) = 1/D(i,j);
            else % 配送点j-1，优先级priority(j-1)
                eta(i,j) = (1/D(i,j)) * priority(j-1);
            end
        end
    end
end
eta(isinf(eta)) = 0; % 处理距离为0的情况

%% 5. 信息素初始化（文档3.2.3节TSP模型）
tau = ones(city_num, city_num);
tau = tau / city_num; % 均匀初始化

%% 6. 迭代优化（核心算法，含容错机制）
best_route = zeros(max_iter, city_num); % 重命名：原best_path改为best_route
best_dist = inf(max_iter, 1);          % 每代最优距离

% 记录每代是否包含11号节点（10号配送点）
include_10th_dp = zeros(max_iter, 1);

for iter = 1:max_iter
    routes = zeros(m, city_num);       % 重命名：原paths改为routes
    dists = zeros(m, 1);               % 蚂蚁距离
    
    for ant = 1:m
        visited = false(1, city_num);
        visited(1) = true;             % A点(1)标记为已访问
        routes(ant, 1) = 1;            % 明确路径起点为1，避免0出现
        current = 1; % 添加：初始化当前节点
    
        for i = 2:city_num
            allowed = find(~visited);  % 未访问节点索引
            
            % 核心过滤：强制排除0索引（防御性编程）
            allowed = allowed(allowed >= 1);
            if isempty(allowed)
                error('错误：无可选有效节点，路径生成失败');
            end
            
            % 计算选择概率（含鲁棒性处理）
            p = zeros(1, length(allowed));
            for j = 1:length(allowed)
                next_city = allowed(j);
                tau_ij = tau(current, next_city);
                eta_ij = eta(current, next_city);
                if tau_ij > 0 && eta_ij > 0
                    p(j) = tau_ij^alpha * eta_ij^beta;
                else
                    p(j) = 1e-10;  % 保底概率，防止全零
                end
            end
            
            % 概率归一化（处理全零情况）
            sum_p = sum(p);
            if sum_p == 0
                p = ones(size(p)) / length(p); % 强制均匀分布
            else
                p = p / sum_p;
            end
            p = p + 1e-10;  % 防止浮点误差
            
            % 轮盘赌选择（含容错）
            r = rand();
            cumsum_p = cumsum(p);
            next_idx = find(cumsum_p >= r, 1);
            
            if isempty(next_idx)
                next_idx = 1;  % 保底策略：选第一个节点
                fprintf('迭代=%d, 蚂蚁=%d, 步骤=%d：概率全零，选择第一个节点\n', iter, ant, i);
            end
            
            next_city = allowed(next_idx);
            % 新增：二次校验next_city合法性
            if next_city < 1 || next_city > city_num
                error('错误：选择的节点索引非法：%d', next_city);
            end
            routes(ant, i) = next_city;
            visited(next_city) = true;
            current = next_city;% 更新当前节点
        end
        
        % 回到A点（文档3.2.1节假设3）
        routes(ant, city_num) = 1;
        visited(1) = true;  % 标记A为已访问（避免后续误操作）
        
        % 计算路径总距离（含索引校验）
        dist = 0;
        for i = 1:city_num
            from = routes(ant, i);
            if i == city_num
                to = 1;
            else
                to = routes(ant, i+1);
            end
            
            % 强化校验：先检查0索引，再检查范围
            if from == 0 || to == 0
                error('错误：路径中出现非法索引0，from=%d, to=%d', from, to);
            end
            if ~(from >= 1 && from <= city_num && to >= 1 && to <= city_num)
                error('索引超出合法范围：from=%d, to=%d', from, to);
            end
            dist = dist + D(from, to);
        end
         dists(ant) = dist; % 添加：保存蚂蚁路径距离
    end
    
    % 更新当代最优解
    [min_dist, min_idx] = min(dists);
    if min_dist < best_dist(iter)
        best_dist(iter) = min_dist;
        best_route(iter, :) = routes(min_idx, :);  % 重命名：原best_path改为best_route
    else
        best_dist(iter) = best_dist(iter-1);
        best_route(iter, :) = best_route(iter-1, :);  % 重命名
    end
    
    % 关键修改：修复11号节点路径完整性
    if ~any(best_route(iter, :) == 11)  % 11是10号配送点的节点索引
        fprintf('迭代%d: 修复10号配送点路径\n', iter);
        best_route(iter, :) = repair_route(best_route(iter, :), 11, D);
        best_dist(iter) = calculate_route_distance(best_route(iter, :), D);
    end
    include_10th_dp(iter) = any(best_route(iter, :) == 11);

    % 信息素更新（文档3.3.3节模型）
    tau = (1 - rho) * tau;
    for ant = 1:m
        route = routes(ant, :);  % 重命名：原path改为route
        dist = dists(ant);
        for i = 1:city_num
            from = route(i);
            % 在访问 route(i+1) 之前先判断 i 是否为 city_num
            if i == city_num
                to = 1;
            else
                to = route(i+1);
            end
            % 强化校验：先检查0索引，再检查范围
            if from == 0 || to == 0
                error('错误：路径中出现非法索引0，from=%d, to=%d', from, to);
            end
            if ~(from >= 1 && from <= city_num && to >= 1 && to <= city_num)
                error('索引超出合法范围：from=%d, to=%d', from, to);
            end
            tau(from, to) = tau(from, to) + Q / dist;
        end
    end
end

%% 7. 提取全局最优解
[global_min_dist, global_min_idx] = min(best_dist);
global_best_route = best_route(global_min_idx, :);  % 重命名：原global_best_path改为global_best_route

% 强制校验：路径必须以A为起点和终点
if global_best_route(1) ~= 1 || global_best_route(end) ~= 1
    error('全局最优路径未正确闭合！当前首尾：%d → %d', ...
          global_best_route(1), global_best_route(end));
end
%% 路径生成后校验（添加在迭代优化后）
disp('=== 路径完整性校验 ===');
disp(['路径长度：', num2str(length(global_best_route))]);  % 应为22
disp(['唯一节点数：', num2str(length(unique(global_best_route)))]);  % 应为22
if length(unique(global_best_route)) < 22
    warning('路径缺失节点！请调整算法参数或距离矩阵。');
end

    % 最终路径完整性校验与修复
if ~any(global_best_route == 11)
    global_best_route = repair_route(global_best_route, 11, D);
    global_min_dist = calculate_route_distance(global_best_route, D);
end
%% 8. 路径与满意度计算（文档3.3.3节满意度函数）
fprintf('最优路径链：');
for k = 1:city_num
    idx = global_best_route(k);
    if idx == 1
        fprintf('A');
    else
        fprintf('%d', idx-1);  % 配送点编号=索引-1
    end
    if k < city_num
        fprintf(' -> ');
    end
end
fprintf('\n总路程：%.2f 米\n', global_min_dist);

%% 8. 满意度计算（修正单位与窗口）
speed = 120;  % 无人车速度：提高到120米/分钟
satisfaction = 0;
route_full = [global_best_route, 1];
arrival_time = 0;

for k = 1:length(route_full)-1  
    from = route_full(k);
    to = route_full(k+1);
    distance = D(from, to);
    arrival_time = arrival_time + distance / speed;  % 距离→时间（分钟）
    
    if from > 1  % 仅计算配送点（跳过A）
        dp_num = from - 1;  % 配送点1~21对应节点2~22
        if dp_num >= 1 && dp_num <= 21  % 防御性检查
            order = orders(dp_num);    
            
            % 调整时间窗口（更宽松）
            lower = 5 + (order / 100) * 15;  % 订餐量越大，窗口越晚开始
            upper = 70 - (order / 100) * 15;  % 订餐量越大，窗口越早结束
            
            if arrival_time < lower
                s = 1;
            elseif arrival_time <= upper
                s = 1 - (arrival_time - lower) / (upper - lower);
            else
                s = 0;
            end
            satisfaction = satisfaction + s;
        end
    end
end
% 计算满意度百分比
satisfaction_percentage = satisfaction / 21 * 100; 
fprintf('总满意度：%.2f%%\n', satisfaction_percentage);
fprintf('本次订餐量（配送点1-21）：');
disp(orders)

%% 9. 路径可视化（A点绿色标注）
figure('Position', [100, 100, 800, 600]);
theta = linspace(0, 2*pi, city_num);
% 增大半径，这里将半径设为 2，你可以根据实际情况调整
radius = 2; 
x = radius * cos(theta); 
y = radius * sin(theta);

% 手动调整A点坐标到右侧（避免与其他节点重合）
x(1) = 1.2 * radius;  % A点X轴右移，同时乘以半径
y(1) = 0;    % A点Y轴居中

hold on;

% 绘制节点：区分A点和配送点
for k = 1:city_num
    if k == 1  % A点（节点1）
        % 绿色节点，更大尺寸突出显示
        plot(x(k), y(k), 'go', 'MarkerSize', 14, 'MarkerFaceColor', 'g');  
        % 标注“A”，上移0.2*radius单位避免遮挡
        text(x(k), y(k)+0.2*radius, 'A', ...  
             'HorizontalAlignment', 'center', 'VerticalAlignment', 'bottom', ...
             'FontSize', 12, 'Color', 'green');
    else  % 配送点（节点2~22）
        plot(x(k), y(k), 'bo', 'MarkerSize', 12, 'MarkerFaceColor', 'b');
        dp_label = num2str(k-1);  % 节点k对应配送点k-1
        text(x(k), y(k), dp_label, ...
             'VerticalAlignment', 'bottom', 'HorizontalAlignment', 'right', ...
             'FontSize', 9);
    end
end

% 假设当前节点为 current_node，你可以根据实际情况修改这个值
current_node = 1; 

% 绘制路径箭头（使用已闭合的全局最优路径）
route_full = global_best_route;  % 已确保首尾为A，无需重复添加
for i = 1:length(route_full)-1
    from = route_full(i);
    to = route_full(i+1);
    
    % 防御性检查：确保索引合法
    if from < 1 || from > city_num || to < 1 || to > city_num
        error('路径节点非法：from=%d, to=%d', from, to);
    end
    
    % 根据起始和终止节点设置不同颜色
    if to == current_node
        arrow_color = [1 0 0 0.7]; % 红色
    elseif from == current_node
        arrow_color = [0.3 0 0.3 0.7]; % 深紫色
    end
    
    quiver(x(from), y(from), x(to)-x(from), y(to)-y(from), 0, ...
           'LineWidth', 1.2, 'MaxHeadSize', 0.2, 'Color', arrow_color);
end

title('基于文档1模型的最优配送路径', 'FontSize', 12);
axis equal; grid on;
xlabel('X坐标'); ylabel('Y坐标');
% 扩展X轴和Y轴范围，根据半径调整
xlim([-1.5*radius 1.5*radius]);  
ylim([-1.2*radius 1.2*radius]);  